
#include <robust_formation/FormationCtrl.h>//0818
#include <cppad/ipopt/solve.hpp>
#include <Eigen/Core>
#include <iostream>
#include <math.h>
#include "ros/ros.h"
using CppAD::AD;

class FG_eval {
public:
    Eigen::VectorXd vec_cmd;
    typedef CPPAD_TESTVECTOR( AD<double> ) ADvector;
    FG_eval(Eigen::VectorXd vec_cmd) {
        this->vec_cmd = vec_cmd;
    }

    void operator()(ADvector& fg, const ADvector& x)
    {
        //  assert( fg.size() == 3 );
        // assert( x.size()  == 2 );
        // vec_cmd << ref_distance, ref_angle, leader_robot_pos_x, leader_robot_pos_y, other_robot_pos_x, other_robot_pos_y;
        // Fortran style indexing
        AD<double> x1 = x[0];
        AD<double> x2 = x[1];
        // f(x)
        fg[0] = (x1 - vec_cmd[0])*(x1 - vec_cmd[0]) + (x2 - vec_cmd[1])*(x2 - vec_cmd[1]);
        // g_1 (x)
        fg[1] = ((vec_cmd[2] - x1 * CppAD::sin(x2)) - (vec_cmd[4])) * ((vec_cmd[2] - x1 * CppAD::sin(x2)) - (vec_cmd[4]))
              + ((vec_cmd[3] - x1 * CppAD::cos(x2)) - (vec_cmd[5])) * ((vec_cmd[3] - x1 * CppAD::cos(x2)) - (vec_cmd[5]));
        return;
    }
};

Formation_MPC::Formation_MPC() {
ROS_INFO("===============检查点0: init===============");
}

Formation_MPC::~Formation_MPC() {

}

vector<double> Formation_MPC::Solve(Eigen::VectorXd state, Eigen::VectorXd vec_cmd) {
    ROS_INFO("===============检查点1: Solve===============");
    bool ok = true;
    size_t i;
    typedef CPPAD_TESTVECTOR( double ) Dvector;

    // number of independent variables (domain dimension for f and g)
    size_t nx = 2;
    // number of constraints (range dimension for g)
    size_t ng = 1;
    // initial value of the independent variables
    Dvector xi(nx);
    xi[0] = state[0];
    xi[1] = state[1];
    // lower and upper limits for x
    Dvector xl(nx), xu(nx);
    xl[0] = 0.30;
    xu[0] = 2*sqrt(2);
    xl[1] = 0.0;
    xu[1] =  2 * M_PI;
    // lower and upper limits for g
    Dvector gl(ng), gu(ng);
    gl[0] = 0.25;     gu[0] = 1.0e19;
    // gl[0] = 5.0;     gu[0] = 1.0e19;
    // gl[1] = -1.0e19;     gu[1] = 1.0e19;

    // object that computes objective and constraints
    FG_eval fg_eval(vec_cmd);

    // options
    std::string options;
    // turn off any printing
    options += "Integer print_level  0\n";
    options += "String  sb           yes\n";
    // maximum number of iterations
    options += "Integer max_iter     10\n";
    // approximate accuracy in first order necessary conditions;
    // see Mathematical Programming, Volume 106, Number 1,
    // Pages 25-57, Equation (6)
    options += "Numeric tol          1e-6\n";
    // derivative testing
    options += "String  derivative_test            second-order\n";
    // maximum amount of random pertubation; e.g.,
    // when evaluation finite diff
    options += "Numeric point_perturbation_radius  0.\n";

    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;

    // solve the problem
    CppAD::ipopt::solve<Dvector, FG_eval>(
            options, xi, xl, xu, gl, gu, fg_eval, solution
    );
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;

    // std::cout << "l: " << solution.x[0] << "theta: " << solution.x[1] << std::endl;

    vector<double> result;
    result.push_back(solution.x[0]);
    result.push_back(solution.x[1]);
    return result;
}
